//serial_imu.cpp
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include <unistd.h>
#include <sensor_msgs/Imu.h>
#include <serial_imu/NAV100_0x14.h>
#include <serial_imu/NAV100_0x1F.h>
#include <serial_imu/NAV100_data_package.h>
#include <serial_imu/NAV100_0x1C.h>
#include <serial_imu/NAV100_raw_package.h>

#include <signal.h>

#include <Eigen/Geometry>

//https://www.cnblogs.com/Braveliu/p/12219521.html
//用在C与C++混合编写的代码中，意为：如果是在C++下编译的
#ifdef __cplusplus 
extern "C"{     //指示编译器大括号内这部分代码按C语言（而不是C++）的方式进行编译
                //https://www.cnblogs.com/xiangtingshen/p/10980055.html
#endif

#include <stdint.h>
#include <string.h>
#include <stdbool.h>

#include "ch_serial.h"

#define IMU_SERIAL   "/dev/ttyUSB0"
//#define BAUD         (115200)
#define BAUD         (250000)
#define BUF_SIZE     100


void publish_imu_data(raw_t *data, sensor_msgs::Imu *imu_data);
void publish_0x14_data(raw_t *data, serial_imu::NAV100_0x14 *data_imu);
void publish_0x1F_data(raw_t *data, serial_imu::NAV100_0x1F *data_imu);
//void publish_0x1C_data(raw_t *data, serial_imu::NAV100_0x1C *data_imu);
void publish_0x1C_data(raw_t &data, serial_imu::NAV100_0x1C &data_imu, sensor_msgs::Imu &imu_data);

#ifdef __cplusplus
}
#endif

static raw_t raw;

static int frame_rate;
static int frame_count;

static uint8_t buf[2048];

void timer(int sig)
{
	if(SIGALRM == sig)
	{
		frame_rate = frame_count;
		frame_count = 0;
        // alarm超时则产生SIGALRM信号。
        // https://blog.csdn.net/qq_22863733/article/details/80349120
		alarm(1);
	}
}

int main(int argc, char** argv)
{
	int rev = 0;
    //初始化节点
	ros::init(argc, argv, "serial_imu");
    //声明节点句柄
	ros::NodeHandle n;

    //发布主题
	ros::Publisher IMU_pub = n.advertise<sensor_msgs::Imu>("/imu/raw_data", 20);
//	ros::Publisher Imu_0x91_pub = n.advertise<serial_imu::Imu_0x91_msg>("/imu_0x91_package", 10);
//	ros::Publisher Imu_0x62_pub = n.advertise<serial_imu::Imu_0x62_msg>("/imu_0x62_package", 10);
    ros::Publisher Imu_0x14_pub = n.advertise<serial_imu::NAV100_0x14>("/nav100_0x14_package", 10);
//    ros::Publisher Imu_0x1F_pub = n.advertise<serial_imu::NAV100_0x1F>("/nav100_0x1F_package", 10);
    ros::Publisher Imu_0x1C_pub = n.advertise<serial_imu::NAV100_0x1C>("/nav100_0x1C_package", 10);


    //声明串口对象
	serial::Serial sp;

    ///设置串口属性
	sp.setPort(IMU_SERIAL);     //设置接口
	sp.setBaudrate(BAUD);       //设置波特率
    //创建并设置超时时间timeout  https://www.cnblogs.com/visionfeng/p/5614066.html
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
	sp.setTimeout(to);

    //alarm超时则产生SIGALRM信号。
    //在SIGALRM被激活时，会调用timer()函数
	signal(SIGALRM,timer);

	///打开串口
	try
	{
		sp.open();
	}
	catch(serial::IOException& e)
	{
		ROS_ERROR_STREAM("Unable to open port.");
		return -1;
	}

    //判断串口是否打开成功
	if(sp.isOpen())
	{
	    //https://www.codeleading.com/article/67245272933/
        ROS_INFO_STREAM(IMU_SERIAL << " is opened.");
	}
	else
	{
		return -1;
	}

    // alarm超时则产生SIGALRM信号。 由于计算数据包频率
    // https://blog.csdn.net/qq_22863733/article/details/80349120
	alarm(1);

    // 希望发布信息的速度为500Hz，要和 loop_rate.sleep() 配合使用
    // https://blog.csdn.net/weixin_51060040/article/details/122238461
    ros::Rate loop_rate(500);
    sensor_msgs::Imu imu_data;
	serial_imu::NAV100_0x14 imu_0x14_msg;
//	serial_imu::NAV100_0x1F imu_0x1F_msg;
	serial_imu::NAV100_0x1C imu_0x1C_msg;

	while(ros::ok())
	{
        //获取缓冲区内的字节数
		size_t num = sp.available();
		if(num!=0)
		{
			uint8_t buffer[BUF_SIZE]; 
	
			if(num > BUF_SIZE)
				num = BUF_SIZE;

            //读出数据
			num = sp.read(buffer, num);

			if(num > 0)
			{
//				imu_data.header.stamp = ros::Time::now();
				imu_data.header.frame_id = "odom";

//                imu_0x14_msg.header.stamp = ros::Time::now();
                imu_0x14_msg.header.frame_id = "base_0x14_link";

//                imu_0x1F_msg.header.stamp = ros::Time::now();
//                imu_0x1F_msg.header.frame_id = "base_0x1F_link";

//                imu_0x1C_msg.header.stamp = ros::Time::now();
                imu_0x1C_msg.header.frame_id = "base_0x1C_link";

				for (int i = 0; i < num; i++)
				{
                    imu_data.header.stamp = ros::Time::now();
                    imu_0x14_msg.header.stamp = ros::Time::now();
//                    imu_0x1F_msg.header.stamp = ros::Time::now();
                    imu_0x1C_msg.header.stamp = ros::Time::now();

//                    //16进制的方式打印到屏幕
//                    std::cout << std::hex << (buffer[i] & 0xff) << " ";

                    // 只有读完一整个数据包之后才会使rev=1，继续进行操作；
                    // 否则rev都等于0，继续读下一帧数据
					rev = ch_serial_input(&raw, buffer[i]);
                    if(rev) {
                        frame_count++;
                        if (raw.item_code[raw.nitem_code - 1] == ID28) {
                            publish_0x1C_data(raw, imu_0x1C_msg, imu_data);
                            Imu_0x1C_pub.publish(imu_0x1C_msg);
                            IMU_pub.publish(imu_data);
                        } else if (raw.item_code[raw.nitem_code - 1] == ID20) {
                            publish_0x14_data(&raw, &imu_0x14_msg);

                            Imu_0x14_pub.publish(imu_0x14_msg);

                            // 手上的IMU ID20数据包加速数据为0， 加上这句会使 imu_data 的也为0，所以注释掉
                            // 若IMU正常可考虑恢复
//                            publish_imu_data(&raw, &imu_data);
//                            IMU_pub.publish(imu_data);
                        }

//                        else if (raw.item_code[raw.nitem_code - 1] == IMU31){
//                            Imu_0x1F_pub.publish(imu_0x1F_msg);
//                            publish_0x1F_data(&raw, &imu_0x1F_msg);
//                        }
                        else {
                            frame_count--;
                            ROS_ERROR("Unreconiged IMU!");
                        }
                    }
				}
			}
		}
		loop_rate.sleep();
	}
    
	sp.close();
 
	return 0;
}

void imu_data_package(raw_t *data, serial_imu::NAV100_data_package *data_imu, int num)
{
	data_imu->tag = data->item_code[num];

	data_imu->frame_rate = frame_rate;

	data_imu->id = data->imu[num].id;

	data_imu->time = data->imu[num].timestamp;

//	data_imu->prs = data->imu[num].pressure;
    data_imu->Latitude = data->imu[num].Latitude;
    data_imu->Longitude = data->imu[num].Longitude;
    data_imu->Height = data->imu[num].Height;

    data_imu->v_north = data->imu[0].v_world[0];
    data_imu->v_east = data->imu[0].v_world[1];
    data_imu->v_down = data->imu[0].v_world[2];

    data_imu->acc_x = data->imu[num].acc[0];
    data_imu->acc_y = data->imu[num].acc[1];
    data_imu->acc_z = data->imu[num].acc[2];

    data_imu->gravity = data->imu[0].gravity;

    data_imu->eul_r = data->imu[0].eul[0];
    data_imu->eul_p = data->imu[0].eul[1];
    data_imu->eul_y = data->imu[0].eul[2];

    data_imu->gyr_x = data->imu[num].gyr[0];
    data_imu->gyr_y = data->imu[num].gyr[1];
    data_imu->gyr_z = data->imu[num].gyr[2];

    data_imu->Latitude_dev = data->imu[0].deviation[0];
    data_imu->Longitude_dev = data->imu[0].deviation[1];
    data_imu->Height_dev = data->imu[0].deviation[2];


	data_imu->quat_x = data->imu[num].quat[0];
	data_imu->quat_y = data->imu[num].quat[1];
	data_imu->quat_z = data->imu[num].quat[2];
    data_imu->quat_w = data->imu[num].quat[3];
}
//void imu_data_package(raw_t *data, serial_imu::NAV100_raw_package *data_imu, int num)
void imu_data_package(raw_t &data, serial_imu::NAV100_raw_package &data_imu, sensor_msgs::Imu &imu_data, int num)
{
    data_imu.tag = data.item_code[num];
    data_imu.frame_rate = frame_rate;
    data_imu.id = data.imu[num].id;
    data_imu.time = data.imu[num].timestamp;


    data_imu.acc_x = data.imu[num].acc[0];
    data_imu.acc_y = data.imu[num].acc[1];
    data_imu.acc_z = data.imu[num].acc[2];
    data_imu.gyr_x = data.imu[num].gyr[0];
    data_imu.gyr_y = data.imu[num].gyr[1];
    data_imu.gyr_z = data.imu[num].gyr[2];
    data_imu.quat_x = data.imu[num].quat[0];
    data_imu.quat_y = data.imu[num].quat[1];
    data_imu.quat_z = data.imu[num].quat[2];
    data_imu.quat_w = data.imu[num].quat[3];


    imu_data.linear_acceleration.x = data.imu[num].acc[0];
    imu_data.linear_acceleration.y = data.imu[num].acc[1];
    imu_data.linear_acceleration.z = data.imu[num].acc[2];
    imu_data.angular_velocity.x = data.imu[num].gyr[0];
    imu_data.angular_velocity.y = data.imu[num].gyr[1];
    imu_data.angular_velocity.z = data.imu[num].gyr[2];
    imu_data.orientation.x = data.imu[num].quat[0];
    imu_data.orientation.y = data.imu[num].quat[1];
    imu_data.orientation.z = data.imu[num].quat[2];
    imu_data.orientation.w = data.imu[num].quat[3];
}


void publish_imu_data(raw_t *data, sensor_msgs::Imu *imu_data)
{
//    Eigen::Quaterniond q = Eigen::Quaterniond(Eigen::AngleAxisd(data->imu[data->nimu - 1].eul[0], Eigen::Vector3d::UnitZ()) *
//                            Eigen::AngleAxisd(data->imu[data->nimu - 1].eul[1], ::Eigen::Vector3d::UnitY()) *
//                            Eigen::AngleAxisd(data->imu[data->nimu - 1].eul[2], ::Eigen::Vector3d::UnitX()));

    imu_data->orientation.x = data->imu[data->nimu - 1].quat[0];
	imu_data->orientation.y = data->imu[data->nimu - 1].quat[1];
	imu_data->orientation.z = data->imu[data->nimu - 1].quat[2];
	imu_data->orientation.w = data->imu[data->nimu - 1].quat[3];
	imu_data->angular_velocity.x = data->imu[data->nimu - 1].gyr[0];
	imu_data->angular_velocity.y = data->imu[data->nimu - 1].gyr[1];
	imu_data->angular_velocity.z = data->imu[data->nimu - 1].gyr[2];
	imu_data->linear_acceleration.x = data->imu[data->nimu - 1].acc[0];
	imu_data->linear_acceleration.y = data->imu[data->nimu - 1].acc[1];
	imu_data->linear_acceleration.z = data->imu[data->nimu - 1].acc[2];
}

void publish_0x14_data(raw_t *data, serial_imu::NAV100_0x14 *data_imu)
{
    Eigen::Quaterniond q = Eigen::Quaterniond(Eigen::AngleAxisd(data->imu->eul[0], Eigen::Vector3d::UnitX()) *
                                              Eigen::AngleAxisd(data->imu->eul[1], ::Eigen::Vector3d::UnitY()) *
                                              Eigen::AngleAxisd(data->imu->eul[2], ::Eigen::Vector3d::UnitZ()));

    data->imu->quat[0] = q.x();
    data->imu->quat[1] = q.y();
    data->imu->quat[2] = q.z();
    data->imu->quat[3] = q.w();
    imu_data_package(data, &(data_imu->imu_data),data->nimu - 1);
}

// TODO
void publish_0x1F_data(raw_t *data, serial_imu::NAV100_0x1F *data_imu)
{

}


void publish_0x1C_data(raw_t &data, serial_imu::NAV100_0x1C &data_imu, sensor_msgs::Imu &imu_data)
{
    Eigen::Quaterniond q = Eigen::Quaterniond(Eigen::AngleAxisd(data.imu->eul[0], Eigen::Vector3d::UnitX()) *
                                              Eigen::AngleAxisd(data.imu->eul[1], ::Eigen::Vector3d::UnitY()) *
                                              Eigen::AngleAxisd(data.imu->eul[2], ::Eigen::Vector3d::UnitZ()));

    data.imu->quat[0] = q.x();
    data.imu->quat[1] = q.y();
    data.imu->quat[2] = q.z();
    data.imu->quat[3] = q.w();
    imu_data_package(data, data_imu.imu_data, imu_data, data.nimu - 1);
}
